import numpy as np
import acado
import time
import matplotlib.pyplot as plt
from numpy import sin, cos,exp

import time
import rospy
import math
pc_size=30

N=20 #timestep
NU=2
NOD=pc_size*3+3
NX=4
NY=6
NYN=4

NW=6
NWN=4

pc_W=np.ones([3,pc_size])*10+np.random.uniform(-0.1,0.1,(3,pc_size))
pc_W[0,:]=np.random.uniform(-1,1,(1,pc_size))*0.1+8
pc_W[1,:]=np.random.uniform(-1,1,(1,pc_size))
pc_W[2,:]=np.random.uniform(-1,1,(1,pc_size))

pc_test=np.load('pc_test.npy')
#pc_W[:,:20]=pc_test[:,:20]

plt.plot(pc_W.T)
plt.show()
x0=np.zeros(NX).reshape(1,NX)
od=np.zeros(NOD)
y=np.zeros([N,NY])
u=np.zeros([N,NU])
Q=np.eye(NW)

Q=np.diag([0.1,0.100,100.,1.,1,1])
W=np.transpose(np.tile(Q,N))
WN=Q[:-2,:-2]
X=np.tile(x0.reshape(NX,1),N+1).T
OD=np.ones([N+1,NOD])

OD[:,pc_size*0:pc_size*1]=pc_W[0,:]  #x
OD[:,pc_size*1:pc_size*2]=pc_W[1,:]  #y
OD[:,pc_size*2:pc_size*3]=pc_W[2,:]  #z

pc_W_center=np.mean(pc_W,axis=1)
print(pc_W_center)

xc=pc_W_center[0]
yc=pc_W_center[1]   
zc=pc_W_center[2]

yaw_c=math.atan2(yc,xc)
pitch_c=math.atan2(zc,math.sqrt(xc**2+yc**2))
print(np.rad2deg(yaw_c))
print(np.rad2deg(pitch_c))

X[1:,0]=yaw_c
X[1:,1]=pitch_c

xlog=[]
for j in range(2):
    X,u_=acado.mpc(1,1,x0,X,u,OD,y,W,WN,1)
    x0=X[0,:].reshape(1,NX)
    #OD[:,pc_size*1:pc_size*2]=200-j*20
    xlog.append(x0)
xlog_arr=np.vstack(xlog)
xlog_arr=X

plt.plot(xlog_arr[:,0]/3.14*180)
plt.plot(xlog_arr[:,1]/3.14*180)
plt.legend(["yaw","pitch"])
plt.show()
